/**
 * @file:          EncoderCalibrator.c
 * @brief:
 * @details:
 * @author:        wjh
 * @date created:  2023.08.26
 * @version:       1.0.0
 * @par copyright (c):
 *
 * @par history (desc):
 *   version:1.0.0, wjh, 2023.08.26,21:13:36
 */

/* Include Files **************************************************************/
#include "EncoderCalibrator.h"
#include "base_math.h"

/* Global Variable Define *****************************************************/

/* Function Define ************************************************************/

void EncoderCalibrator_Init(struct EncoderCalibrator *self)
{
    self->Run = EncoderCalibrator_Run;
    self->FourierSeries = EncoderCalibrator_FourierSeries;

    self->encoder2rad = BM_2PI / self->param_encoder_max_pulse;
    self->rad2encoder = self->param_encoder_max_pulse / BM_2PI;
    for (uint16_t i = 0; i < MAX_NUM_OF_FOURIER_SERIES; i++)
    {
        // if (fabsf(self->coeffs[i].A) > MIN_AMP)
        // {
        self->num_of_item++;
        // }
    }
}

int32_t EncoderCalibrator_Run(struct EncoderCalibrator *self, int32_t encoder)
{
    int32_t out;
    if (self->param_en != 0)
    {
        float x = encoder * self->encoder2rad;
        float y = x + self->FourierSeries(self, x);
        out = (int32_t)(y * self->rad2encoder);
    }
    else
    {
        out = encoder;
    }

    return out;
}

float EncoderCalibrator_FourierSeries(struct EncoderCalibrator *self, float theta)
{
    float out = 0;

    for (uint16_t i = 0; i < self->num_of_item; i++)
    {
        out += self->coeffs[i].A * cos_bm(self->coeffs[i].f * theta + self->coeffs[i].phi);
    }

    return out;
}
